import roslib; roslib.load_manifest('framer')
import rospy
import image_geometry
from geometry_msgs.msg import PointStamped
from sensor_msgs.msg import CameraInfo
from framer.msg import Status
import tf
from faces.msg import Faces
import smach

class FaceProcessor(smach.State):
   def __init__(self):
      smach.State.__init__(self, outcomes=['next', 'abort'], output_keys=['faces', 'cam_model'])
      self.cam_model = None
      self.cam_frame = None
      self.resolution = None
      self.faces = None

      self.tf = tf.TransformListener() 
      sub = rospy.Subscriber("/wide_stereo/right/camera_info", CameraInfo, self.cam_callback)
      sub2 = rospy.Subscriber("/faces/data", Faces, self.face_callback)

      self.status_pub = rospy.Publisher('/framer/status', Status)
      
   def cam_callback(self, cam_info):
      if not self.cam_model == None:
         return
      self.resolution = (cam_info.width, cam_info.height)
      self.cam_frame = cam_info.header.frame_id
      self.cam_model = image_geometry.PinholeCameraModel()
      self.cam_model.fromCameraInfo(cam_info)
      self.center = None
      
   def face_callback(self, faces):
      if self.cam_model == None:
         rospy.loginfo("Camera information not yet available, skipping.")

      self.faces = faces


   def execute(self, userdata):
      self.status_pub.publish(Status(Status.FINDING_FACES))
      rospy.loginfo("Processing faces.")
      while (self.cam_model == None) and (self.faces == None) and (not rospy.is_shutdown()):
         rospy.sleep(0.1)  # Basically, wait until all data is available, then proceed. 
                           # rospy.shutdown() thing just allows it to exit gracefully when ctrl-c is pressed

      if rospy.is_shutdown():
         return 'abort'

      userdata.cam_model = self.cam_model
      userdata.faces = self.faces
      return 'next'
      
